

class JointController():
    """
    A simple PD controller for a robot joint. 

                     -----------------------
      [q_ref]  --->  |                     |
      [q,qd,ff] ---> |   JointController   | ---> tau
      [Kp,Kd]  --->  |                     |
                     -----------------------

    """
    # Declare output port for logging
    def __init__(self,q_ref,q,qd,ff,Kp,Kd):
        self.q_ref = q_ref

        self.q = q
        self.v = qd
        self.ff = ff
        self.Kp = Kp
        self.Kd = Kd
        self.tau = 0

    def updateStatus(self,q_ref,q,qd,ff):
        self.q_ref = q_ref
        self.q = q
        self.v = qd
        self.ff = ff

    def updateParams(self,Kp,Kd)
        self.Kp = Kp
        self.Kd = Kd

    def controlLaw(self):
        q_err = q_ref - q
        v_err = 0 - v
        tau = ff + Kp*q_err + Kd*v_err

        return tau




